#include "board.h"

int angle_pid_pwm, left_pwm, right_pwm;
float angle,tp_angle;
PID angle_pid;


void angle_pid_init(void)
{
    pid_init(&angle_pid,130, 1, 3, 100, 1500, 0 );
}



//进行角度的控制
void angle_control(float target_angle)
{
    tp_angle = get_angle();
    angle = target_angle - tp_angle;

    //防止角度突变
    if((angle < 180)&&(angle > -180))
    {
        angle = angle;
    }
    else if(angle >= 180)
    {
        angle = angle - 360;
    }
    else if(angle <= -180)
    {
        angle = angle + 360;
    }
    
    left_pwm = Car_Angle_Base_Speed - 35*angle;
    right_pwm = Car_Angle_Base_Speed + 35*angle;
    set_all_motor(left_pwm, right_pwm);//根据输出结果输出速度    

}